/*
 PololuFunctions from
 http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1231053939
 Pololu micro serial servo controller modes
 Software Serial from Arduino example by Tom Igoe
 put() (now position_absolute) & set_speed() functions found on pololu forums & modified
 mike crist 2009-01-03
*/

#include <SoftwareSerial.h>
#include <ctype.h>

#include <QTRSensors.h>

#define bit9600Delay 84  
#define halfBit9600Delay 42
#define bit4800Delay 188


#define halfBit4800Delay 94

#define NoNl     false
#define Nl       true



#define rstPin 11
#define rxPin  12
#define txPin  13

#define sensorActivatePin 10
#define no_of_sensors 6
#define thresholdSecurity 200
#define showSensorVals     true
#define notShowSensorVals  false

#define leftMotorPin  6
#define rightMotorPin 5

#define no_of_messungen 3

#define motorOfflineSpeed 150
#define motorMaxSpeed     200

int threshold[no_of_sensors];

byte sensorPin[] = {9, 8, 7, 3, 2, 4};
unsigned int sensor[no_of_sensors];
int realSensorVal[no_of_sensors];
// create an object for six QTR-xRC sensors on digital pins 9, 8, 5, 4, 3, 2
PololuQTRSensorsRC qtrrc((unsigned char[]) {9, 8, 7, 3, 2, 4}, no_of_sensors);

#define backlogNumber 10
int measurement[backlogNumber];
int mPointer=0;

char ch;
char soft_ch;

int  RXval;

byte heliostatNo=0;

// set up a new serial port
SoftwareSerial softSerial =  SoftwareSerial(rxPin, txPin);

byte correctTaurusOn=false;
byte actServoPos=120;
int  targetSensorVal=-1;

long loopcounter=0;
long relLoop;

byte middlePos=100;

byte motorSpeed=70;

int  lastSensorValFound=25;
int  sensorValFound=25;

void setup()
{
  
  
  digitalWrite(leftMotorPin, LOW);
  pinMode(leftMotorPin, OUTPUT);
  analogWrite(leftMotorPin, 0);
 
  digitalWrite(rightMotorPin, LOW);
  pinMode(rightMotorPin, OUTPUT);
  analogWrite(rightMotorPin, 0); 
  
  
  pinMode(rxPin, INPUT);
  digitalWrite(txPin, HIGH); // keeps pololu board from getting spurious signal
  pinMode(txPin, OUTPUT);
  
  digitalWrite(rstPin, LOW); //resets the Pololu board
  pinMode(rstPin, OUTPUT);

  // set the data rate for the hardware serial port
  Serial.begin(9600);
  // set the data rate for the SoftwareSerial port
  softSerial.begin(9600);
  
  pinMode(sensorActivatePin, OUTPUT);
  digitalWrite(sensorActivatePin, HIGH);
  
  for (int i=0;i<no_of_sensors;i++) {
    pinMode(sensorPin[i], INPUT);
  }
  
  setPaperThreshold();
 
  //pololu should be reset by now
  digitalWrite(rstPin, HIGH);
  delay(1000);
  position_7bit(0, 10);
  heliostatNo=7;
  //set_speed(1, heliostatNo);
  
 
 
 targetSensorVal=25;
 byte mByte=130;
 //= findLeftRightRange(qtrrc, targetSensorVal);
       
 position_8bit(heliostatNo, mByte);
 actServoPos=mByte;
 
 correctTaurusOn=true;
  
  
  analogWrite(leftMotorPin,  motorSpeed);
  analogWrite(rightMotorPin,  motorSpeed); 
  
  
}

void loop()
{
  
  
  static int v = 0;
  static byte vByte=0;
  loopcounter++;
  
  
    if (correctTaurusOn) {
    
    sensorValFound=readAndNormalize(qtrrc,notShowSensorVals);
    if (sensorValFound>=0) {        
        measurement[mPointer]=sensorValFound;
        mPointer=(mPointer+1)%backlogNumber;
    }
    
   
    
    if (sensorValFound<0) {
      
       //von der Linie abgekommen 
       byte set2Pos=211;  
       if (isRightOfLine()) {
         set2Pos=71; 
         analogWrite(leftMotorPin,  100);
         analogWrite(rightMotorPin,  255);
       } else {
         analogWrite(leftMotorPin,  255);
         analogWrite(rightMotorPin, 100);
       }       
       position_8bit(heliostatNo, set2Pos);
       RXval = SWread();
       delay(2);
       if (RXval==0 || RXval==1) resetPololu(20); 
       
       
    } else {
      
       //Reguläre Fahrt
       byte incVal=0;      
       int offTarget=abs(sensorValFound-targetSensorVal);
       if      (offTarget >15)      {
                                     incVal=3; 
                                     motorSpeed=constrain(motorSpeed-3,motorOfflineSpeed,motorMaxSpeed);
       }       
       else if (offTarget >10)       incVal=2;
       else if (offTarget >5 )      {
                                     incVal=1;
                                     motorSpeed=constrain(motorSpeed+10,motorOfflineSpeed,motorMaxSpeed); 
       }
     
       if (sensorValFound>targetSensorVal)  actServoPos+=incVal;
       else                                 actServoPos-=incVal;
        
       
       actServoPos=constrain(actServoPos,80,190);
       //motorSpeed+=constrain(motorSpeed+3,0,motorMaxSpeed);
     
           
        analogWrite(leftMotorPin,  motorSpeed);
        analogWrite(rightMotorPin,  motorSpeed);
     
      
      //showln("actServoPos:",actServoPos);    
      position_8bit(heliostatNo, actServoPos);
      RXval = SWread();
      delay(2);
      if (RXval==0 || RXval==1) resetPololu(10);      
      
    }
    
    }  
    
    
    
  
  
  if (Serial.available()) {    
    ch = Serial.read();
    switch(ch) {
      case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 'a':
        show("az:",v,Nl); 
        position_absolute(heliostatNo, v);            
        v = 0;
        break;
      case 'e':
        show("elv:",v,Nl);        
        //setServo(elevation(heliostatNo),v);        
        v = 0;
        break;
      case 'h':
        heliostatNo=v;
        show("helNo:",heliostatNo);
        v=0;
        break;
        
      case 'g':
       show("left Motor; val",v);
       analogWrite(leftMotorPin, v); 
       v=0;
       break;
       
       case 'j':       
       show("right Motor; val",v);
       analogWrite(rightMotorPin, v); 
       v=0;
       break; 
       
       case 'm':
       showLastMeasurements();
       break;
       
       case 'o':
       if (correctTaurusOn) correctTaurusOn=false; 
       else                 correctTaurusOn=true;
       break;
       
      case 'p':
        vByte= findLeftRightRange(qtrrc, v);
        showln("bestPosition:",vByte);
        position_8bit(heliostatNo, vByte);         
        //RXval = SWread();
        //showln("last RXVal0:",RXval);         
        v=0;
        break;
       
      case 'P':
        vByte= findLeftRightRange(qtrrc, v);
        showln("bestPosition:",vByte);
        position_8bit(heliostatNo, vByte);
        actServoPos=vByte;
        targetSensorVal=v;
        correctTaurusOn=true;
        delay(100);
        v=0;
       break;
      case 'Q':
        correctTaurusOn=false;
        break;
      case 'r':
        resetPololu(20);
        break;
      case 's':
        showln("position_7bit 0,",v);
        vByte=v;
        position_7bit(heliostatNo, vByte);        
        RXval = SWread();
        showln("RXVal0:",RXval);        
       v=0;
       break;
       case 't':
        showln("position_8bit 0,",v);
        vByte=v;
        position_8bit(heliostatNo, vByte);         
        RXval = SWread();
        showln("RXVal0:",RXval);        
       v=0;
       break;     
      case 'v':
        showln("speed 0,",v);
        set_speed(0, v);
        v=0;
        break;
        
       case 'T':
       Serial.println("Test");
       break; 
     case 'q':
        readAndNormalize(qtrrc,showSensorVals);  
        break;    
     case 'z':
       moveSensorUntilOff(v, qtrrc);
       v=0;
       break;  
    case 'Z':
        setThreshold();
      break;   
       
    }
  }           
  
  

}


/*
show() is used to send the name and value of a variable
to serial. The third parameter is used to inicate whether 
it should be finished with newline or not.
*/
void show(char nameStr[], long val) {
     show(nameStr,val, NoNl);
}

void showln(char nameStr[], long val) {
     show(nameStr,val, Nl);
}

void show(char nameStr[], long val, boolean newline) {
  Serial.print(nameStr);  
  if (newline)
       Serial.println(val);
  else Serial.print(val);
}

void show(char nameStr[], long pos,  long val, boolean newline) {
       
      show(nameStr, pos, ":", val, newline);
}

void show(char nameStr[], long pos, char rightStr[],long val, boolean newline) {
  Serial.print(nameStr);  
  Serial.print(pos);
  Serial.print(rightStr);
  if (newline)       
       Serial.println(val);
  else Serial.print(val);
}


void showLastMeasurements() {
       Serial.println("lastMeasurements");
      for (int mPointerIdx=mPointer;
               mPointerIdx+backlogNumber>mPointer;
               mPointerIdx--) { 
             int modPos=(mPointerIdx+backlogNumber-1)%backlogNumber;
             show(" mp[",mPointerIdx,"]=",measurement[modPos],NoNl);
           }
      Serial.println(" ");
}


boolean isRightOfLine() {       
    int lastmPointer=(mPointer+backlogNumber-1)%backlogNumber;
    return (measurement[lastmPointer]<25);
}

int measureMiddle() {
  
  int maxMeasurement=-1;
  int minMeasurement=50;
  int localmPointer=mPointer;
   for (int i=0;i<backlogNumber;i++) {
        minMeasurement=min(minMeasurement,measurement[localmPointer]);
        maxMeasurement=max(maxMeasurement,measurement[localmPointer]);        
        localmPointer=(localmPointer+1)%backlogNumber;
   }
   
  if (minMeasurement<0) {
      if (maxMeasurement>30) minMeasurement=60; 
  }
   
  return (maxMeasurement+minMeasurement)/2; 
  
}

void showMeasurement() {
   Serial.print("messungen:");
   int localmPointer=mPointer;
   for (int i=0;i<backlogNumber;i++) {
        Serial.print(measurement[localmPointer]);
        Serial.print("\t");
        localmPointer=(localmPointer+1)%backlogNumber;
   }
   Serial.print("\n");
}


void setPaperThreshold() {
      
threshold[0]=65;
threshold[1]=236;
threshold[2]=236;
threshold[3]=200;
threshold[4]=236;
threshold[5]=200;     
}



void setThreshold() {  
  
  for (int i=0;i<no_of_sensors;i++) {
            threshold[i]=0;
       }
  
  for (int i=0;i<30;i++) {
      myRead(qtrrc);
      for (int i=0;i<no_of_sensors;i++) {
         if (sensor[i]>threshold[i]) {
            threshold[i]=sensor[i];           
         }
       }   
  }
 
  for (int i=0;i<no_of_sensors;i++) {
            show("threshold[",i,threshold[i],Nl);
   }
  
}

/*myRead reads the sensor Input */
void myRead(PololuQTRSensorsRC qtr) {
  
  digitalWrite(sensorActivatePin, HIGH);
  qtr.read(sensor);
  digitalWrite(sensorActivatePin, LOW);
  
}

int correctTaurus(PololuQTRSensorsRC qtr, int targetSensorVal) {
  
      
    int sensorValFound=readAndNormalize(qtr,notShowSensorVals);
    
    byte incVal=0;
    
    int offTarget=abs(sensorValFound-targetSensorVal);
    
    if (sensorValFound>0) {
        if (offTarget >10)       incVal=5;
        else if (offTarget >5 )  incVal=1;        
    }
    
    /*
    if (sensorValFound>0) 
    if      (abs(sensorValFound-targetSensorVal) >15)  incVal=15;
    else if (abs(sensorValFound-targetSensorVal) >10)  incVal=10;
    else if (abs(sensorValFound-targetSensorVal) >6)  incVal=5;
    else if (abs(sensorValFound-targetSensorVal) >4)  incVal=2;
    else if (abs(sensorValFound-targetSensorVal) >2 )  incVal=1;
    */
    
    if (incVal>0) 
        if (sensorValFound>targetSensorVal)  actServoPos+=incVal;
        else                                 actServoPos-=incVal;
    
    
    actServoPos=constrain(actServoPos,80,190);  
    
    return sensorValFound;
  
}

byte findLeftRightRange(PololuQTRSensorsRC qtr, int seekSensorVal) {
     
  
      int sensorValFound=-10;
      int bestAbstand=100;
      byte bestFoundPosition=0;
      position_8bit(heliostatNo, 20); 
      delay(300);
      
      for (byte position=20;position<=230;position=position+10) {
        position_8bit(heliostatNo, position); 
        delay(30);
        sensorValFound=readAndNormalize(qtr,notShowSensorVals);
        if (sensorValFound>=0 && abs(sensorValFound-seekSensorVal)<bestAbstand) {
            bestAbstand=abs(sensorValFound-seekSensorVal);
            bestFoundPosition=position;
        }        
      }
      
     return bestFoundPosition;
}

void moveSensorUntilOff(int left, PololuQTRSensorsRC qtr ) {
     position_8bit(heliostatNo, left);         
     //RXval = SWread();
     delay(200);
     position_8bit(heliostatNo, 220);
     boolean contin=true;
     int rounds=0;
     for (;rounds<300 && contin;rounds++) {
       contin = (readAndNormalize(qtr,notShowSensorVals) >=0);
     }    
    showln("rounds:",rounds);
}


int readAndNormalize(PololuQTRSensorsRC qtr, boolean showVals) {
  
  digitalWrite(sensorActivatePin, HIGH);
  qtr.read(sensor);  
  digitalWrite(sensorActivatePin, LOW);
  
  //realSensorValuedetermine the maximum and its position  
  for (int i=0;i<no_of_sensors;i++) {         
         int tempSensor=sensor[i]-threshold[i]-thresholdSecurity; 
         realSensorVal[i]=constrain(tempSensor,0,5000);
         if (showVals) show("\ts",i,realSensorVal[i],NoNl);
  }      
  if (showVals) Serial.println(" ");
  int pos = position(realSensorVal);
  if (showVals) showln("realpos:",pos);
  return pos;
}

long position(int realSensorVal[]) {
  
    long minPositivePos=no_of_sensors;
    long maxPositivePos=-1;
    for (int i=0;i<no_of_sensors;i++) {
         if (realSensorVal[i]>0) {
             if (i>maxPositivePos) maxPositivePos=i;
             if (i<minPositivePos) minPositivePos=i;
         }
    }
    
    long realPos=10*(minPositivePos*realSensorVal[minPositivePos]+
                 maxPositivePos*realSensorVal[maxPositivePos])/
                 (realSensorVal[minPositivePos]+realSensorVal[maxPositivePos]);
    
    return (int) realPos;
  
}


void set_servo_parameters(byte servo, byte rangeVal)
{
   //this function uses pololu mode command 0 to set servo parameters
   //servo is the servo number (typically 0-7)
   //rangeVal of 15 gives 180 deg in 8-bit, 90 deg in 7 bit

   byte temp;
   byte parameters;

   temp = 1 << 6;			   //set first two bits of parameters (on = 1, forward = 0)
   temp = temp + (rangeVal & 0x1f);   //put first five bits of rangeVal into temp
   parameters = temp & 0x7f;	    //take only bottom 7 bits

   //Send a Pololu Protocol command
   softSerial.print(0x80,BYTE);	 //start byte
   softSerial.print(0x01,BYTE);	 //device id
   softSerial.print(0x00,BYTE);	 //command number
   softSerial.print(servo,BYTE);	//servo number
   softSerial.print(parameters,BYTE); //parameters
}

void set_speed(byte servo, byte speedVal)
{
   //this function uses pololu mode command 1 to set speed
   //servo is the servo number (typically 0-7)
   //speedVal is servo speed (1=slowest, 127=fastest, 0=full)
   //set speedVal to zero to turn off speed limiting

   speedVal = speedVal & 0x7f; //take only lower 7 bits of the speed

   //Send a Pololu Protocol command
   softSerial.print(0x80,BYTE);     //start byte
   softSerial.print(0x01,BYTE);     //device id
   softSerial.print(0x01,BYTE);     //command number
   softSerial.print(servo,BYTE);    //servo number
   softSerial.print(speedVal,BYTE); //speed
}

void position_7bit(byte servo, byte posValue)
{
  //this function uses pololu mode command 2 to set position
  //servo is the servo number (typically 0-7)
  //posValue * range (set with command 0) adjusted by neutral (set with command 5)
  //determines servo position

   byte pos = posValue & 0x7f;     //get lower 7 bits of position

   //Send a Pololu Protocol command
   softSerial.print(0x80,BYTE);     //start byte
   softSerial.print(0x01,BYTE);     //device id
   softSerial.print(0x02,BYTE);     //command number
   softSerial.print(servo,BYTE);    //servo number
   softSerial.print(pos, BYTE);     //position
}

byte wrapped_8bit(byte servo, byte posValue) {
     int readVal=0;
     byte count=5;
     while (readVal<100 && count>0) {
        position_8bit(servo, posValue);
        readVal=SWread();
        if (count<2) resetPololu(20);
        count--;
     }
    return readVal; 
}

void position_8bit(byte servo, byte posValue)
{
  //this function uses pololu mode command 3 to set position
  //servo is the servo number (typically 0-7)
  //posValue * range (set with command 0) adjusted by neutral (set with command 5)
  //determines servo position

   byte temp;
   byte pos_hi,pos_low;

   temp = posValue & 0x80;	//get bit 8 of position
   pos_hi = temp >> 7;		//shift bit 8 by 7
   pos_low = posValue & 0x7f;     //get lower 7 bits of position

   //Send a Pololu Protocol command
   softSerial.print(0x80,BYTE);     //start byte
   softSerial.print(0x01,BYTE);     //device id
   softSerial.print(0x03,BYTE);     //command number
   softSerial.print(servo,BYTE);    //servo number
   softSerial.print(pos_hi, BYTE);  //bits 8 thru 13
   softSerial.print(pos_low, BYTE); //bottom 7 bits
}

void position_absolute(byte servo, int angle)
{
  //this function uses pololu mode command 4 to set absolute position
  //servo is the servo number (typically 0-7)
  //angle is the absolute position from 500 to 5500

   unsigned int temp;
   byte pos_hi,pos_low;

   temp = angle & 0x1f80;  //get bits 8 thru 13 of position
   pos_hi = temp >> 7;     //shift bits 8 thru 13 by 7
   pos_low = angle & 0x7f; //get lower 7 bits of position

   //Send a Pololu Protocol command
   softSerial.print(0x80, BYTE);    //start byte
   softSerial.print(0x01, BYTE);    //device id
   softSerial.print(0x04, BYTE);    //command number
   softSerial.print(servo, BYTE);   //servo number
   softSerial.print(pos_hi, BYTE);  //bits 8 thru 13
   softSerial.print(pos_low, BYTE); //bottom 7 bits
}

void set_neutral(byte servo, int angle)
{
  //this function uses pololu mode command 5 to set neutral position
  //servo is the servo number (typically 0-7)
  //angle is the absolute position from 500 to 5500

   unsigned int temp;
   byte pos_hi,pos_low;

   temp = angle & 0x1f80;  //get bits 8 thru 13 of position
   pos_hi = temp >> 7;     //shift bits 8 thru 13 by 7
   pos_low = angle & 0x7f; //get lower 7 bits of position

   //Send a Pololu Protocol command
   softSerial.print(0x80, BYTE);    //start byte
   softSerial.print(0x01, BYTE);    //device id
   softSerial.print(0x05, BYTE);    //command number
   softSerial.print(servo, BYTE);   //servo number
   softSerial.print(pos_hi, BYTE);  //bits 8 thru 13
   softSerial.print(pos_low, BYTE); //bottom 7 bits
} 

int SWread()
{
  byte val = 0;
  //without countdown sometimes endless loop
  long countdown=500;
  while (digitalRead(rxPin)&&countdown-->0);
  //wait for start bit
  if (digitalRead(rxPin) == LOW) {
    delayMicroseconds(halfBit9600Delay);
    for (int offset = 0; offset < 8; offset++) {
     delayMicroseconds(bit9600Delay);
     val |= digitalRead(rxPin) << offset;
    }
    //wait for stop bit + extra
    delayMicroseconds(bit9600Delay); 
    delayMicroseconds(bit9600Delay);
    if (countdown==0) val=0;
    return val;
  }
}

void resetPololu(int delayVal) {
  digitalWrite(rstPin, LOW);
  if (delayVal>0) delay(delayVal);
  digitalWrite(rstPin, HIGH);  
}

